void init_msensor()
{
  int i;
  uint8_t attr_dynamiclist[] = ATTR_DYNAMICLIST;
  uint8_t attr_hardwarelist[] = ATTR_HARDWARELIST;
#ifndef TOSSIM
  uint8_t attr_samplelist[] = ATTR_SAMPLELIST;
  struct {
    uint8_t attr_index, min_samples;
  } attr_minsamplelist[] = ATTR_MINSAMPLELIST;
#endif

  memset(attr_desc, 0, sizeof(attr_desc));

  for (i = 0; i < sizeof(attr_dynamiclist)/sizeof(attr_dynamiclist[0]); i++)
    attr_desc[attr_dynamiclist[i]].is_dynamic = 1;
  for (i = 0; i < sizeof(attr_hardwarelist)/sizeof(attr_hardwarelist[0]); i++)
    attr_desc[attr_hardwarelist[i]].is_hardware = 1;
    
  update_attr(ATTR_ID, TOS_NODE_ID, 0);  
  update_attr(ATTR_MAINTIMER, DEFAULT_TIMER1INTERVAL, 0);

#ifndef TOSSIM
  for (i = 0; i < sizeof(attr_samplelist)/sizeof(attr_samplelist[0]); i++)
    sample_attr(attr_samplelist[i]);
  for (i = 0; i < sizeof(attr_minsamplelist)/sizeof(attr_minsamplelist[0]); i++)
    attr_desc[attr_minsamplelist[i].attr_index].min_samples = attr_minsamplelist[i].min_samples;  
#endif

  summarydesc_count = 0;
  memset(summary_desc, 0, sizeof(summary_desc));

  memset(&eval_info, 0, sizeof(eval_info));

  memset(attr_queue, 0, sizeof(attr_queue));
  attrqueue_first = 0; attrqueue_last = 0; attrqueue_size = 0; 
}

uint8_t stack_pop(STACK *s, TOKEN *t)
{
  if (s->stack_len > 0)
  {
    memcpy(t, &s->stack[--s->stack_len], sizeof(TOKEN));
    return 1;
  } else {
    DBGERR("Cannot pop from empty stack!");
    pause_sim();
    return 0;
  }
}

uint8_t find_idsummary()
{
  int i;

  for (i = 0; i < summarydesc_count; i++)
    if (summary_desc[i].attrdesc_index == ATTR_ID)
      return i;

  return 0;
}

uint8_t find_repairsummary()
{
  int i;

  if (config.repair_level == REPAIR_L2)
    for (i = 0; i < summarydesc_count; i++)
      if (summary_desc[i].attrdesc_index == ATTR_ID && summary_desc[i].options != 0)
        return i;

  return find_idsummary();
}

#ifdef TOSSIM
int sensor_offlinesample(uint32_t sample_time, ATTRIBUTE *value, uint8_t jsel)
{                            // retrieves a reading for time "sample_time" for the current node from a file, or random
  FILE *f;
  char s[256];
  uint32_t t;
  ATTRIBUTE v;

  snprintf(s, sizeof(s), "%s%d/node%d.txt", config.sample_dir, jsel, TOS_NODE_ID);
  f = fopen(s, "r");
  if (f == NULL)
  {
    DBGERR("Cannot find sample data file %s", s);
    return -1;
  }

  while (!feof(f))
  {
    fscanf(f, "%ld %hu\n", &t, &v);
    if (t < sample_time)
      *value = v;
    else break;
  }

  fclose(f);
  return 0;
}
#endif

int32_t doeval_stack(uint8_t need_result)
{
  TOKEN t;
  uint8_t op_id;
  int32_t temp1, temp2;
  int i;

  memcpy(&t, &eval_info.s->stack[--eval_info.stack_ofs], sizeof(TOKEN));
  switch (t.type)
  {
    case TOKEN_OP:
      switch (OPGET_TYPE(t.value))
      {                                     // functions permitting shortcut evaluation
        case OP_AND:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 != 0));
                            return (temp1 && temp2);

        case OP_OR:         temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 == 0));
                            return (temp1 || temp2);

        case OP_MUL:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 != 0));
                            return (temp1 * temp2);

        case OP_DIV:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 != 0));
                            return ((temp1 != 0) ? ((temp2 != 0) ? (temp1 / temp2) : 0xffff) : 0);

        case OP_MOD:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 != 0));
                            return ((temp1 != 0) ? ((temp2 != 0) ? (temp1 % temp2) : 0xffff) : 0);

        case OP_SHL:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 > 0));
                            return ((temp1 > 0) ? (temp1 << temp2) : 0);

        case OP_SHR:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result && (temp1 > 0));
                            return ((temp1 > 0) ? (temp1 >> temp2) : 0);

        case OP_IN:         temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = 0;
                            for (i = 0; i < OPGET_EXTRAARG(t.value); i++)
                            {
                              temp2 |= (temp1 == doeval_stack(need_result && (temp2 == 0)));
                              if (eval_info.need_sampling)
                                return 0;
                            }
                            return temp2;

        case OP_IF:         temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            if (temp1)
                            {
                              temp1 = doeval_stack(need_result);
                              if (eval_info.need_sampling)
                                return 0;
                              doeval_stack(0);
                            } else {
                              doeval_stack(0);
                              if (eval_info.need_sampling)
                                return 0;
                              temp1 = doeval_stack(need_result);
                            }
                            return temp1;
                                                                // functions not permitting shortcut evaluation

        case OP_XOR:        temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return ((temp1 != 0) ^ (temp2 != 0));

        case OP_EQ:         temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 == temp2);

        case OP_GR:         temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 > temp2);

        case OP_LESS:       temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 < temp2);

        case OP_GREQ:       temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 >= temp2);

        case OP_LESSEQ:     temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 <= temp2);

        case OP_PLUS:       temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 + temp2);

        case OP_MINUS:      temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return (temp1 - temp2);

        case OP_DIST:       temp1 = doeval_stack(need_result);
                            if (eval_info.need_sampling)
                              return 0;
                            temp2 = doeval_stack(need_result);
                            return int_sqrt(((temp1 & 0xff)-(temp2 & 0xff))*((temp1 & 0xff)-(temp2 & 0xff)) +
                              ((temp1 >> 8)-(temp2 >> 8))*((temp1 >> 8)-(temp2 >> 8)));

        case OP_NOT:        return !doeval_stack(need_result);
        case OP_ABS:        return abs(doeval_stack(need_result));
        case OP_HASH:       return VALUE_SHUFFLE(doeval_stack(need_result));
        case OP_SQRT:       return int_sqrt(doeval_stack(need_result));
        case OP_SQR:        temp1 = doeval_stack(need_result);
                            return (temp1 * temp1);

        case OP_AGGRCOUNT:  doeval_stack(0);                         // aggregate functions
                            return 1;

        case OP_AGGRSUM:    return doeval_stack(need_result);
        case OP_AGGRMIN:    return doeval_stack(need_result);
        case OP_AGGRMAX:    return doeval_stack(need_result);

        default:            DBGERR("Invalid operator: %d", t.value);
                            pause_sim();
                            return 0;
      }

    case TOKEN_NUM:       return t.value;

    case TOKEN_ATTR:      if (need_result == 0)
                            return 0;

                          op_id = ATTRGET_TYPE(t.value);
                          if (ATTRGET_SEQ(t.value))    // timestamp
                            return timercycles_to_seconds(ATTRGET_TEMP(t.value) ? timer_cycles : attr_desc[op_id].sampling_time);
                          else {
                            if (ATTRGET_TEMP(t.value))  // temp
                            {
                              if (ATTRGET_REL(t.value) == REL_S)
                              {
                                if (eval_info.stemp)
                                  return eval_info.stemp[op_id];
                                else {
                                  DBGERR("No s-temp provided");
                                  pause_sim();
                                  return 0;
                                }
                              } else {
                                if (eval_info.ttemp)
                                  return eval_info.ttemp[op_id];
                                else {
                                  DBGERR("No t-temp provided");
                                  pause_sim();
                                  return 0;
                                }
                              }   
                            }  

                            if (eval_info.sample_tolerance > 0 &&
                              (attr_desc[op_id].sample_number == 0 || attr_desc[op_id].sampling_time == 0 ||
                               attr_desc[op_id].sampling_time+eval_info.sample_tolerance < timer_cycles))
                            {
#ifdef TOSSIM
                              DBGOUT("Attribute %s needs sampling", attr_name[op_id]);
#else
                              DBGOUT("Attribute %d needs sampling", op_id);
#endif
                              switch (op_id)
                              {
                                case ATTR_MEM:
                                  update_attr(ATTR_MEM, HEAPMAN_BUFFERSIZE-heapman_usage, timer_cycles);
                                  break;
                                case ATTR_HOPS:
                                  update_attr(ATTR_HOPS, trees[0].hops, timer_cycles);
                                  break;
                                case ATTR_TIME:
                                  update_attr(ATTR_TIME, timercycles_to_seconds(timer_cycles), timer_cycles);
                                  break;
                                case ATTR_RFID:
                                case ATTR_RFIDSIG:                                                                   
                                  //update_attr(ATTR_RFIDSIG, 0, timer_cycles);    // Set RFID strength to 0 after a long time
                                  break;
                                default:
                                  if (attr_desc[op_id].is_hardware)
                                  {
                                    sample_attr(op_id);
                                    eval_info.need_sampling = 1;
                                  }
                                  break;
                              }
                            }

                            return attr_desc[op_id].value;
                          }

    default:              DBGERR("Invalid token type: %d\n", t.type);
                          pause_sim();
                          return 0;
  }
}

int32_t eval_stack(STACK *s, ATTRIBUTE *stemp, ATTRIBUTE *ttemp, uint16_t sample_tolerance)
{
  int32_t result;

#ifdef TOSSIM
  DBGOUT("Evaluating expression:");
  print_stack(s);
#endif

  eval_info.s = s;
  eval_info.stack_ofs = s->stack_len;
  eval_info.stemp = stemp;
  eval_info.ttemp = ttemp;
  eval_info.sample_tolerance = sample_tolerance;
  eval_info.need_sampling = 0;
  
  if (s->stack_len == 0)
  {
    DBGOUT("Default result = 1");
    return 1;
  }

  result = doeval_stack(1);
  DBGOUT("Result = %d, need sampling: %d", result, eval_info.need_sampling);

  return result;
}

uint8_t sensor_needsampling()
{
  int i;

  for (i = 1; i < MAX_ATTRDESC; i++)     // Small optimization: do not consider ATTR_ID (0) here.
    if (attr_desc[i].needs_sampling)
      return 1;

  return 0;
}

void handle_attrupdatemsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  int i;
  ATTRUPDATE_MSGHI msg_in;

  DBGOUT("Attr update message received");
  unmarshal_attrupdatemsghi(&msg_in, data);
  for (i = 0; i < msg_in.node_count; i++)
    if (msg_in.nodes[i].id == TOS_NODE_ID)
      update_attr(msg_in.attr_index, msg_in.nodes[i].value, timer_cycles);
}

uint8_t filterbaseflood_attrupdate(uint8_t *data_len, uint8_t *data)
{
  int i, k = 0;
  ATTRUPDATE_MSGHI msg_in;
  
  unmarshal_attrupdatemsghi(&msg_in, data);
  for (i = 0; i < msg_in.node_count; i++)
    if (summary_test(trees[0].summary[ATTR_ID], msg_in.nodes[i].id, 0))
    {    
      if (i > k)
        memcpy(&msg_in.nodes[k], &msg_in.nodes[i], sizeof(msg_in.nodes[0]));
      k++;  
    } 
    
  msg_in.node_count = k;   
  (*data_len) = marshal_attrupdatemsghi(&msg_in, data);  
  
  return (k == 0);
}

void dohandle_summaryspec(ADDRESS source_node, uint8_t data_len, uint8_t *data)
{
  int i;
  SUMMARYSPEC_MSGHI msg;

  DBGOUT("Summary spec msg received from %d", source_node);
  unmarshal_summaryspecmsghi(&msg, data);

  for (i = 0; i < MAX_ATTRDESC; i++)
  {
    attr_desc[i].is_dynamic = 1-BV_GET(msg.attr_static, i);
    
    if (TOS_NODE_ID == BASESTATION_ADDR)
#ifdef TOSSIM
      DBGOUT("Attribute %s is %s", attr_name[i], attr_desc[i].is_dynamic ? "dynamic" : "static");
#else
      DBGOUT("Attribute %d is %s", i, attr_desc[i].is_dynamic ? "dynamic" : "static");
#endif
  }

  summarydesc_count = msg.summarydesc_count;
  for (i = 0; i < summarydesc_count; i++)
  {
    summary_desc[i].attrdesc_index = msg.summary_desc[i].attrdesc_index;
    summary_desc[i].type = msg.summary_desc[i].type;
    summary_desc[i].options = msg.summary_desc[i].options;
    
    if (TOS_NODE_ID == BASESTATION_ADDR)
#ifdef TOSSIM
      DBGOUT("Summary %d is on attribute %s (%d, %d)", i, attr_name[summary_desc[i].attrdesc_index],
        summary_desc[i].type, summary_desc[i].options);
#else
      DBGOUT("Summary %d is on attribute %d (%d, %d)", i, summary_desc[i].attrdesc_index,
        summary_desc[i].type, summary_desc[i].options);
#endif
  }
}

void handle_summaryspecmsghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)
{
  dohandle_summaryspec(source_node, data_len, data);
  alloc_treesummaries();
}

void update_attr(uint8_t attr_index, ATTRIBUTE value, uint32_t sampling_time)
{
  attr_desc[attr_index].value = value;
  attr_desc[attr_index].sample_number++;
  attr_desc[attr_index].sampling_time = sampling_time;
  attr_desc[attr_index].is_sampling = 0;  

  if (attr_desc[attr_index].min_samples == 0 || 
    (attr_desc[attr_index].sample_number % attr_desc[attr_index].min_samples) == 0)
  {
    attr_desc[attr_index].needs_sampling = 0;
        
#ifdef TOSSIM
    DBGOUT("Attribute %s updated with value %d", attr_name[attr_index], value);
    log_querysample(attr_index, value);
#else
    DBGOUT("Attribute %d updated with value %d", attr_index, value);
#endif
  } else attr_desc[attr_index].needs_sampling = 1;
}

void sample_attr(uint8_t attr_index)
{
  if (attr_desc[attr_index].is_sampling == 0)
  {
#ifdef TOSSIM
    DBGOUT("Scheduled sampling for attribute %s", attr_name[attr_index]);
#else
    DBGOUT("Scheduled sampling for attribute %d", attr_index);
#endif
    attr_desc[attr_index].needs_sampling = 1;
  }
}

uint8_t attrqueue_push(uint8_t attr_index, ATTRIBUTE value, uint8_t group_next)
{
  if (attrqueue_size == ATTRQUEUE_SIZE)
  {
    DBGERR("Attribute queue full");
    attrqueue_pop();
  }

  DBGOUT("Enqueued value %u for attribute %hu, group next: %hu", value, attr_index, group_next);
  attr_queue[attrqueue_last].attr_index = attr_index;
  attr_queue[attrqueue_last].value = value;
  attr_queue[attrqueue_last].group_next = group_next;
  attr_queue[attrqueue_last].sampling_time = timer_cycles;
  
  attrqueue_last = (attrqueue_last+1) % ATTRQUEUE_SIZE;
  attrqueue_size++;

  return 0;
}

uint8_t attrqueue_pop()
{
  uint8_t group_next;

  if (attrqueue_size == 0)
  {
    DBGERR("Attribute queue empty");
    return 1;
  }

  DBGOUT("Dequeueing attribute values");
  group_next = 1;
  while (attrqueue_size > 0 && group_next)
  {
    update_attr(attr_queue[attrqueue_first].attr_index, attr_queue[attrqueue_first].value,
      attr_queue[attrqueue_first].sampling_time);
    group_next = attr_queue[attrqueue_first].group_next;
    attrqueue_first = (attrqueue_first+1) % ATTRQUEUE_SIZE;
    attrqueue_size--;
  }

  return 0;
}

uint8_t attrqueue_check(uint8_t *attr_mask)
{
  int i;
  
  for (i = 0; i < ATTRQUEUE_SIZE; i++)
    if (BV_GET(attr_mask, attr_queue[i].attr_index))
      return 1;
      
  return 0;  
}

